#if BUOLA_PLATFORM_LINUX

#include <buola/cv/ccloud.h>
#include <buola/cv/trees/cflanntree.h>
#include <buola/cv/registration/icp.h>
#include <buola/app/ccmdline.h>
#include <buola/image/io.h>
#include <buola/scene/cscene.h>
#include <buola/scene/geodes/cpointcloud.h>
#include <buola/scene/transform/cmattransform.h>
#include <buola/scene/cperspectivecamera.h>
#include <buola/scene/csceneview.h>
#include <buola/scene/controllers/ccameracontroller.h>
#include <buola/gui/ctoolkit.h>
#include <buola/image/cgc.h>

using namespace buola;

int main(int pNArg,char **pArgs)
{
    buola_init(pNArg,pArgs);
    
    try 
    {
        
        if(cmd_line().ArgCount()<2)
        {
            msg_info() << "please specify filenames\n";
            return 1;
        }
        
        cvi::CCloud_rgb8 lSrcOrig=cvi::load_cloud(io::file(cmd_line().Arg(0)));
        cvi::CCloud_rgb8 lDstOrig=cvi::load_cloud(io::file(cmd_line().Arg(1)));
        cvi::CCloud_rgb8 lSrc=cvi::downsample(cvi::load_cloud(io::file(cmd_line().Arg(0))),10);
        cvi::CCloud_rgb8 lDst=cvi::downsample(cvi::load_cloud(io::file(cmd_line().Arg(1))),10);
        
        msg_info() << lSrc.size() << "," << lDst.size() << "\n";

        cvi::CFlannTree<double> lDstTree(lDst);
        
        mat::CMat34d lGuess{0.981617,0.0322576,0.188115,-0.0181609,
                            -0.0331149,0.999451,0.00141574,-0.0164767,
                            -0.187966,-0.00761912,0.982146,-0.0304846};
        
        start_timer();
        mat::CMat34d lFinal=cvi::icp(lSrc,lDstTree,lGuess);
        end_timer();

        gui::get_toolkit()->GetOpenGLGC()->MakeCurrent();
        scene::PScene lOldScene=new scene::CScene;
        
        scene::PPointCloud lOldDstCloud=new scene::CPointCloud;
        lOldDstCloud->SetCloud(clone(lDstOrig));
        lOldScene->World()->AddObject(lOldDstCloud);
                
        scene::PPointCloud lOldSrcCloud=new scene::CPointCloud;
        lOldSrcCloud->SetCloud(clone(lSrcOrig));
        scene::PMatTransform lOldTransform=new scene::CMatTransform(lGuess);
        lOldSrcCloud->AttachTo(lOldTransform);
        lOldScene->World()->AddChild(lOldTransform);
        scene::PPerspectiveCamera lOldCamera=new scene::CPerspectiveCamera;
        lOldCamera->SetFOV(2*atan2(240.0,525.0));
        lOldCamera->SetClipping(0.1,50.0);
        lOldCamera->LookAt({0,0,1},{0,0,0},{0,-1,0});

        scene::PSceneView lOldSceneView=new scene::CSceneView;
        lOldSceneView->SetCamera(lOldCamera);
        lOldSceneView->AddScene(lOldScene);
        lOldSceneView->AddController(new scene::CCameraController(lOldCamera));
        lOldSceneView->SetTitle(L"before");
        lOldSceneView->Map(CSize_d(800,600));
            
        scene::PScene lNewScene=new scene::CScene;
        
        scene::PPointCloud lNewDstCloud=new scene::CPointCloud;
        lNewDstCloud->SetCloud(clone(lDstOrig));
        lNewScene->World()->AddObject(lNewDstCloud);
                
        scene::PPointCloud lNewSrcCloud=new scene::CPointCloud;
        lNewSrcCloud->SetCloud(clone(lSrcOrig));
        scene::PMatTransform lNewTransform=new scene::CMatTransform(lFinal);
        lNewSrcCloud->AttachTo(lNewTransform);
        lNewScene->World()->AddChild(lNewTransform);
        scene::PPerspectiveCamera lNewCamera=new scene::CPerspectiveCamera;
        lNewCamera->SetFOV(2*atan2(240.0,525.0));
        lNewCamera->SetClipping(0.1,50.0);
        lNewCamera->LookAt({0,0,1},{0,0,0},{0,-1,0});

        scene::PSceneView lNewSceneView=new scene::CSceneView;
        lNewSceneView->SetCamera(lNewCamera);
        lNewSceneView->AddScene(lNewScene);
        lNewSceneView->AddController(new scene::CCameraController(lNewCamera));
        lOldSceneView->SetTitle(L"after");
        lNewSceneView->Map(CSize_d(800,600));
            
        buola_main_loop();
    }
    catch(std::exception &pE)
    {
        msg_info() << "exception caught: " << pE.what() << "\n";
    }
    
    return buola_finish();
}

#else
int main() {}
#endif
